{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <style>\n",
       "        .output_wrapper, .output {\n",
       "            height:auto !important;\n",
       "            max-height:100000px; \n",
       "        }\n",
       "        .output_scroll {\n",
       "            box-shadow:none !important;\n",
       "            webkit-box-shadow:none !important;\n",
       "        }\n",
       "        </style>\n",
       "    "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#format the book\n",
    "from __future__ import division, print_function\n",
    "%matplotlib inline\n",
    "import sys\n",
    "sys.path.insert(0, '..')\n",
    "import book_format\n",
    "book_format.set_style()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Interactions\n",
    "\n",
    "This is a collection of interactions, mostly from the book. If you have are reading a print version of the book, or are reading it online via Github or nbviewer you will be unable to run the interactions.\n",
    "\n",
    "So I have created this notebook. Here is how you run an interaction if you do not have IPython installed on your computer.\n",
    "\n",
    "1. Go to try.juptyer.org in your browser. It will launch a temporary notebook server for you.\n",
    "\n",
    "2. Click the **New** button and select `Python 3`. This will create a new notebook that will run Python 3 for you in your browser.\n",
    "\n",
    "3. Copy the entire contents of a cell from this notebook and paste it into a 'code' cell in the notebook on your browser. \n",
    "\n",
    "4. Press CTRL+ENTER to execute the cell.\n",
    "\n",
    "5. Have fun! Change code. Play. Experiment. Hack.\n",
    "\n",
    "Your server and notebook is not permanently saved. Once you close the session your data is lost. Yes, it says it is saving your file if you press save, and you can see it in the directory. But that is just happening in a Docker container that will be deleted as soon as you close the window. Copy and paste any changes you want to keep to an external file.\n",
    "\n",
    "Of course if you have IPython installed you can download this notebook and run it on your own computer. Type\n",
    "\n",
    "    ipython notebook\n",
    "    \n",
    "in a command prompt from the directory where you downloaded this file. Click on the name of this file to open it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Experimenting with FPF'\n",
    "\n",
    "\n",
    "The Kalman filter uses the equation $P^- = FPF^\\mathsf{T}$ to compute the prior of the covariance matrix during the prediction step, where P is the covariance matrix and F is the system transistion function. For a Newtonian system $x = \\dot{x}\\Delta t + x_0$ F might look like\n",
    "\n",
    "$$F = \\begin{bmatrix}1 & \\Delta t\\\\0 & 1\\end{bmatrix}$$\n",
    "\n",
    "$FPF^\\mathsf{T}$ alters P by taking the correlation between the position ($x$) and velocity ($\\dot{x}$). This interactive plot lets you see the effect of different designs of F has on this value. For example,\n",
    "\n",
    "* what if $x$ is not correlated to $\\dot{x}$? (set F01 to 0)\n",
    "\n",
    "* what if $x = 2\\dot{x}\\Delta t + x_0$? (set F01 to 2)\n",
    "\n",
    "*  what if $x = \\dot{x}\\Delta t + 2*x_0$? (set F00 to 2)\n",
    "\n",
    "* what if $x = \\dot{x}\\Delta t$?  (set F00 to 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "C:\\Anaconda3\\lib\\site-packages\\IPython\\html.py:14: ShimWarning: The `IPython.html` package has been deprecated since IPython 4.0. You should import from `notebook` instead. `IPython.html.widgets` has moved to `ipywidgets`.\n",
      "  \"`IPython.html.widgets` has moved to `ipywidgets`.\", ShimWarning)\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "609e5b01d9564d6b8c6e025cab5df9cb",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(IntSlider(value=1, description='F00', max=2), FloatSlider(value=1.0, description='F01(dt…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "from IPython.html.widgets import interact, interactive, fixed\n",
    "import IPython.html.widgets as widgets\n",
    "import numpy as np\n",
    "import numpy.linalg as linalg\n",
    "import math\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.patches import Ellipse\n",
    "\n",
    "def plot_covariance_ellipse(x, P, edgecolor='k'):\n",
    "    U,s,v = linalg.svd(P)\n",
    "    angle = math.atan2(U[1,0],U[0,0])\n",
    "    width  = math.sqrt(s[0]) * 2\n",
    "    height = math.sqrt(s[1]) * 2\n",
    "\n",
    "    ax = plt.gca()\n",
    "    e = Ellipse(xy=(0, 0), width=width, height=height, angle=angle,\n",
    "                edgecolor=edgecolor, facecolor='none',\n",
    "                lw=2, ls='solid')\n",
    "    ax.add_patch(e)\n",
    "    ax.set_aspect('equal')\n",
    "    \n",
    "    \n",
    "def plot_FPFT(F00, F01, F10, F11, covar):\n",
    "    \n",
    "    dt = 1.\n",
    "    x = np.array((0, 0.))\n",
    "    P = np.array(((1, covar), (covar, 2)))\n",
    "    F = np.array(((F00, F01), (F10, F11)))\n",
    "\n",
    "    plot_covariance_ellipse(x, P)\n",
    "    plot_covariance_ellipse(x, np.dot(F, P).dot(F.T), edgecolor='r')\n",
    "    #plt.axis('equal')\n",
    "    plt.xlim(-4, 4)\n",
    "    plt.ylim(-4, 4)\n",
    "    plt.title(str(F))\n",
    "    plt.xlabel('position')\n",
    "    plt.ylabel('velocity')\n",
    "                 \n",
    "interact(plot_FPFT, \n",
    "         F00=widgets.IntSlider(value=1, min=0, max=2.), \n",
    "         F01=widgets.FloatSlider(value=1, min=0., max=2., description='F01(dt)'),\n",
    "         F10=widgets.FloatSlider(value=0, min=0., max=2.),\n",
    "         F11=widgets.FloatSlider(value=1, min=0., max=2.),\n",
    "         covar=widgets.FloatSlider(value=0, min=0, max=1.));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Covariance Ellipse\n",
    "\n",
    "See the effect of varying the variances and covariance of a covariance matrix of the form\n",
    "\n",
    "$$\\begin{bmatrix}\\texttt{var}_x & \\texttt{cov}_xy \\\\ \\texttt{cov}_xy & \\texttt{var}_y\\end{bmatrix}$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "94a56f8ac01144e0b32bbb39a353e40a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=5.0, description='var_x', max=20.0), FloatSlider(value=5.0, descriptio…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "from IPython.html.widgets import interact, interactive, fixed\n",
    "from IPython.html.widgets import FloatSlider\n",
    "from math import cos, sin, pi, atan2, sqrt\n",
    "import  numpy.linalg as linalg\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.patches import Ellipse\n",
    "\n",
    "def plot_covariance_ellipse(P):\n",
    "    U,s,v = linalg.svd(P)\n",
    "    angle = atan2(U[1,0],U[0,0])\n",
    "    width  = sqrt(s[0]) * 2\n",
    "    height = sqrt(s[1]) * 2\n",
    "\n",
    "    ax = plt.gca()\n",
    "    e = Ellipse(xy=(0, 0), width=width, height=height, angle=angle,\n",
    "                edgecolor='k', facecolor='none',\n",
    "                lw=2, ls='solid')\n",
    "    ax.add_patch(e)\n",
    "    h, w = height/4, width/4\n",
    "    plt.plot([0, h*cos(angle+pi/2)], [0, h*sin(angle+pi/2)])\n",
    "    plt.plot([0, w*cos(angle)],      [0, w*sin(angle)])\n",
    "\n",
    "def plot_covariance(var_x, var_y, cov_xy):\n",
    "    P = [[var_x, cov_xy], [cov_xy, var_y]]\n",
    "    plot_covariance_ellipse(P)\n",
    "    plt.xlim(-6, 6)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.ylim(-6, 6)\n",
    "    plt.show()\n",
    "\n",
    "interact (plot_covariance,           \n",
    "          var_x=FloatSlider(value=5., min=0, max=20.), \n",
    "          var_y=FloatSlider(value=5., min=0., max=20.), \n",
    "          cov_xy=FloatSlider(value=1.5, min=0.0, max=50, step=.2));\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# g-h Filter\n",
    "\n",
    "Experiment with various values for g-h filter parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "690c1af2d34845dd837f33de97469854",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.0, description='x', max=50.0, min=-50.0), FloatSlider(value=5.0, des…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "from IPython.html.widgets import interact, interactive, fixed\n",
    "import IPython.html.widgets as widgets\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import numpy.random as random\n",
    "\n",
    "def gen_data(x0, dx, count, noise_factor):\n",
    "    return [x0 + dx*i + random.randn()*noise_factor for i in range (count)]\n",
    "\n",
    "def g_h_filter(data, x0, dx, g, h, dt=1., pred=None):    \n",
    "    x = x0\n",
    "    results = []\n",
    "    for z in data:\n",
    "        #prediction step\n",
    "        x_est = x + (dx*dt)\n",
    "        dx = dx        \n",
    "        if pred is not None:\n",
    "            pred.append(x_est)\n",
    "        \n",
    "        # update step\n",
    "        residual = z - x_est\n",
    "        dx = dx    + h * (residual) / dt\n",
    "        x  = x_est + g * residual     \n",
    "        results.append(x)  \n",
    "    return np.array(results)\n",
    "\n",
    "zs = gen_data(x0=5, dx=5, count=100, noise_factor=50)\n",
    "\n",
    "def interactive_gh(x, dx, g, h):\n",
    "    data = g_h_filter(data=zs, x0=x, dx=dx, dt=1.,g=g, h=h)\n",
    "    plt.plot(zs, color='r')\n",
    "    plt.plot(data, color='k')\n",
    "    plt.show()\n",
    "\n",
    "interact (interactive_gh,           \n",
    "          x=widgets.FloatSlider(value=0., min=-50, max=50.), \n",
    "          dx=widgets.FloatSlider(value=5., min=-50., max=50.), \n",
    "          g=widgets.FloatSlider(value=0.1, min=0.01, max=2, step=.02), \n",
    "          h=widgets.FloatSlider(value=0.02, min=0.0, max=0.5, step=0.01));"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
